#include "Precompile.h"
#include "AnimationSampler.h"

#define APPROXIMATION_EPSILON 1.0e-09 
#define VERYSMALL 1.0e-20 
#define MAXIMUM_ITERATIONS 1000

namespace Graphics{
	AnimationSampler::AnimationSampler(std::map<AnimationSamplingType,std::vector<float>>& samplerData, std::map<AnimationSamplingType,unsigned int>& samplerStrides, unsigned int targetJoint, AnimationTargetComponent targetComp):
		m_samplerData(samplerData),m_samplerStrides(samplerStrides),m_targetJoint(targetJoint),m_targetComp(targetComp){
	}

	std::vector<float> AnimationSampler::sample(float time){
		std::vector<float>& inputVector = m_samplerData[AST_INPUT];
		std::vector<float>& interpolateVector = m_samplerData[AST_INTERPOLATION];
		int inputId[2], index = 0;
		std::vector<float> result;

		bool firstSet = false;
		for(unsigned int i=0; i<inputVector.size(); i++){
			if(inputVector[i] <= time){
				firstSet = true;
				inputId[index] = i;
			}
			else if(inputVector[i] >= time){
				index++;
				inputId[index] = i;
			}

			if(index == 1)
				break;
		}

		if(index == 0)
			inputId[1] = inputId[0];
		else if(!firstSet)
			inputId[0] = inputId[1];

		switch(AnimationInterpolationType(int(interpolateVector[inputId[0]]))){
		case AIT_BEZIER:
			interpolateBezier(result,time,inputId);
			break;
		case AIT_STEP:
			break;
		case AIT_LINEAR:
			interpolateLinear(result,time,inputId);
			break;
		default:
			break;
		}

		return result;
	}

	void AnimationSampler::interpolateLinear(std::vector<float>& result, float time, int inputId[2]){
		std::vector<float>& inputVector = m_samplerData[AST_INPUT];
		std::vector<float>& outputVector = m_samplerData[AST_OUTPUT];
		std::vector<float>& interpolateVector = m_samplerData[AST_INTERPOLATION];

		unsigned int stride = m_samplerStrides[AST_OUTPUT];
		bool inBetween = false;
		float distLeft = time-inputVector[inputId[0]];
		float distRight = inputVector[inputId[1]]-time;

		if(distLeft > 0.0000001f && distRight > 0.0000001f){
			inBetween = true;
		}

		if(inBetween){
			Utility::Math::Matrix<4,4> firstMat,secondMat,resultMat;
			Utility::Math::Quaternion<> firstQuat,secondQuat,resultQuat;
			Utility::Math::Vector<> firstTranslate,secondTranslate,resultTranslate;
			
			// Extract matrices
			std::copy(&outputVector[inputId[0]*stride],&outputVector[inputId[0]*stride]+stride,firstMat.m_data);
			std::copy(&outputVector[inputId[1]*stride],&outputVector[inputId[1]*stride]+stride,secondMat.m_data);

			// Disassemble matrices
			firstTranslate(Utility::Math::_x_,firstMat.get(0,3));
			firstTranslate(Utility::Math::_y_,firstMat.get(1,3));
			firstTranslate(Utility::Math::_z_,firstMat.get(2,3));

			secondTranslate(Utility::Math::_x_,secondMat.get(0,3));
			secondTranslate(Utility::Math::_y_,secondMat.get(1,3));
			secondTranslate(Utility::Math::_z_,secondMat.get(2,3));

			firstQuat.createFromMatrix(firstMat);
			secondQuat.createFromMatrix(secondMat);

			// Interpolate parts
			float interpolateValue = (time-inputVector[inputId[0]])/(inputVector[inputId[1]]-inputVector[inputId[0]]);
			resultQuat = firstQuat.slerp(secondQuat,interpolateValue);
			resultTranslate(Utility::Math::_x_,(firstTranslate(Utility::Math::_x_)*interpolateValue)+(secondTranslate(Utility::Math::_x_)*(1.0f-interpolateValue)));
			resultTranslate(Utility::Math::_y_,(firstTranslate(Utility::Math::_y_)*interpolateValue)+(secondTranslate(Utility::Math::_y_)*(1.0f-interpolateValue)));
			resultTranslate(Utility::Math::_z_,(firstTranslate(Utility::Math::_z_)*interpolateValue)+(secondTranslate(Utility::Math::_z_)*(1.0f-interpolateValue)));

			// Put matrix together again
			resultMat.createFromQuaternion(resultQuat);
			resultMat.set(0,3,resultTranslate(Utility::Math::_x_));
			resultMat.set(1,3,resultTranslate(Utility::Math::_y_));
			resultMat.set(2,3,resultTranslate(Utility::Math::_z_));

			result.assign(16,0);
			std::copy(resultMat.m_data,resultMat.m_data+16,result.data());
		}
		else{
			unsigned int ind = 0;

			// Pick frame closest to timestamp
			if(distLeft < distRight)
				ind = inputId[0];
			else
				ind = inputId[1];

			for(unsigned int i=0; i<stride; i++){
				result.push_back(outputVector[ind*stride+i]);
			}
		}
	}

	float AnimationSampler::clampToZeroOne(float value){
		if (value < .0f)
			return .0f;
		else if (value > 1.0f)
			return 1.0f;
		else
			return value;
	}

	/**
	* Returns the approximated parameter of a parametric curve for the value X
	* @param atX At which value should the parameter be evaluated
	* @param P0_X The first interpolation point of a curve segment
	* @param C0_X The first control point of a curve segment
	* @param C1_X The second control point of a curve segment
	* @param P1_x The second interpolation point of a curve segment
	* @return The parametric argument that is used to retrieve atX using the parametric function representation of this curve
	*/
	float AnimationSampler::approximateCubicBezierParameter(float atX, float P0_X, float C0_X, float C1_X, float P1_X){
		if(atX - P0_X < VERYSMALL)
			return 0.0; 
   
		if(P1_X - atX < VERYSMALL)
			return 1.0; 
   
		long iterationStep = 0;
   
		float u = 0.0f; float v = 1.0f; 
   
		//iteratively apply subdivision to approach value atX
		while(iterationStep < MAXIMUM_ITERATIONS){ 
			// de Casteljau Subdivision. 
			float a = (P0_X + C0_X)*0.5f; 
			float b = (C0_X + C1_X)*0.5f; 
			float c = (C1_X + P1_X)*0.5f; 
			float d = (a + b)*0.5f; 
			float e = (b + c)*0.5f; 
			float f = (d + e)*0.5f; //this one is on the curve!
      
			//The curve point is close enough to our wanted atX
			if(fabs(f - atX) < APPROXIMATION_EPSILON) 
				return clampToZeroOne((u + v)*0.5f); 
      
			//dichotomy
			if(f < atX){ 
				P0_X = f; 
				C0_X = e; 
				C1_X = c; 
				u = (u + v)*0.5f; 
			} 
			else{ 
				C0_X = a; C1_X = d; P1_X = f; v = (u + v)*0.5f; 
			}
      
			iterationStep++; 
		}
   
		return clampToZeroOne((u + v)*0.5f);
	}

	void AnimationSampler::interpolateBezier(std::vector<float>& result, float time, int inputId[2]){
		std::vector<float>& inputVector = m_samplerData[AST_INPUT];
		std::vector<float>& outputVector = m_samplerData[AST_OUTPUT];
		std::vector<float>& interpolateVector = m_samplerData[AST_INTERPOLATION];
		std::vector<float>& inTangentVector = m_samplerData[AST_IN_TANGENT];
		std::vector<float>& outTangentVector = m_samplerData[AST_OUT_TANGENT];

		float interpolation = 0;
		float keyDiff = inputVector[inputId[1]]-inputVector[inputId[0]];

		if(keyDiff > 0.0000001f){
			Utility::Math::Vector<float,2> P0,P1,C0,C1,resultPoint;
			P0(Utility::Math::_x_,inputVector[inputId[0]]);
			P0(Utility::Math::_y_,outputVector[inputId[0]]);

			P1(Utility::Math::_x_,inputVector[inputId[1]]);
			P1(Utility::Math::_y_,outputVector[inputId[1]]);

			C0(Utility::Math::_x_,outTangentVector[2*inputId[0]]);
			C0(Utility::Math::_y_,outTangentVector[(2*inputId[0])+1]);

			C1(Utility::Math::_x_,inTangentVector[2*inputId[1]]);
			C1(Utility::Math::_y_,inTangentVector[(2*inputId[1])+1]);

			//interpolation = (time-inputVector[inputId[0]])/keyDiff;
			interpolation = approximateCubicBezierParameter(time,P0(Utility::Math::_x_),C0(Utility::Math::_x_),C1(Utility::Math::_x_),P1(Utility::Math::_x_));

			resultPoint =	(P0*((1-interpolation)*(1-interpolation)*(1-interpolation)))+
							((C0*3)*interpolation*(1-interpolation)*(1-interpolation))+
							((C1*3)*interpolation*interpolation*(1-interpolation))+
							(P1*interpolation*interpolation*interpolation);

			result.push_back(resultPoint(Utility::Math::_y_));
		}
		else{
			result.push_back(outputVector[inputId[0]]);
		}
	}
}